/*******************************************************************************
* Copyright (c) 2008, Jeferson Jose de Miranda                                 *
* UDESC - Universidade do Estado de Santa Catarina                             *
* All rights reserved.                                                         *
*                                                                              *
* Redistribution and use in source and binary forms, with or without           *
* modification, are permitted provided that the following conditions are met:  *
*     * Redistributions of source code must retain the above copyright         *
*       notice, this list of conditions and the following disclaimer.          *
*     * Redistributions in binary form must reproduce the above copyright      *
*       notice, this list of conditions and the following disclaimer in the    *
*       documentation and/or other materials provided with the distribution.   *
*     * Neither the name of the Universidade do Estado de Santa Catarina nor   *
*       the names of its contributors may be used to endorse or promote        *
*       products derived from this software without specific prior written     *
*       permission.                                                            *
*                                                                              *
* THIS SOFTWARE IS PROVIDED BY JEFERSON JOSE DE MIRANDA "AS IS" AND ANY        *
* EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED    *
* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE       *
* DISCLAIMED. IN NO EVENT SHALL JEFERSON JOSE DE MIRANDA BE LIABLE FOR ANY     *
* DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES   *
* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; *
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND  *
* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT   *
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF     *
* THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.            *
*******************************************************************************/

/**
 * \file Point2DMath.hpp
 *
 * \brief Operations between 2D points.
 *
 * \author Jeferson Jose de Miranda
 */

#ifndef POINT2DMATH_HPP
#define POINT2DMATH_HPP

// OpenCv main header.
#include <cv.h>
// Move-In color image.
#include "RGBImage.hpp"

namespace MoveIn
{
    /**
    * \class Point2DMath
    *
    * \brief Calculates common operations between 2D points.
    */
    class Point2DMath
    {
        public:
        static float euclideanDistance(CvPoint p1,
                                       CvPoint p2);

        static CvPoint midPoint(CvPoint p1,
                                CvPoint p2);

        static CvPoint normalize(CvPoint p);

        static float magnitude(CvPoint u);

        static CvPoint2D32f unitVector(CvPoint u);

        static float dotProduct(CvPoint p1,
                                CvPoint p2);

        static float angle(float dot_product);

        static float angle(CvPoint p1,
                           CvPoint p2);

        static float orientedAngle(CvPoint p1,
                                   CvPoint p2);

        static CvPoint sum(CvPoint u, CvPoint v);

        static void rotate(CvPoint *start,
                           CvPoint *end,
                           float angle);

        static void printVector(RGBImage *source,
                                CvPoint start,
                                CvPoint end);
    };

/*******************************************************************************
* MoveIn::Point2DMath::euclideanDistance                                       *
*******************************************************************************/

    float Point2DMath::euclideanDistance(CvPoint p1,
                                         CvPoint p2)
    {
        float result;
        result = (unsigned int) sqrt(pow((p1.x-p2.x),2.0)
                                     +
                                     pow((p1.y-p2.y),2.0));
        return result;
    }

/*******************************************************************************
* MoveIn::Point2DMath::midPoint                                                *
*******************************************************************************/

    CvPoint Point2DMath::midPoint(CvPoint p1,
                                  CvPoint p2)
    {
        CvPoint result;
        result.x = (p1.x + p2.x) / 2;
        result.y = (p1.y + p2.y) / 2;
        return result;
    }

/*******************************************************************************
* MoveIn::Point2DMath::magnitude                                               *
*******************************************************************************/

    float Point2DMath::magnitude(CvPoint u)
    {
        float ux, uy;
        ux = (float) u.x;
        uy = (float) u.y;
        return (sqrt(pow(ux, 2.0) + pow(uy, 2.0)));
    }

/*******************************************************************************
* MoveIn::Point2DMath::unitVector                                              *
*******************************************************************************/

    CvPoint2D32f Point2DMath::unitVector(CvPoint u)
    {
        CvPoint2D32f unit;
        float m;
        unit.x = (float) u.x;
        unit.y = (float) u.y;
        m = magnitude(u);
        unit.x /= m;
        unit.y /= m;
        return unit;
    }

/*******************************************************************************
* MoveIn::Point2DMath::dotProduct                                              *
*******************************************************************************/

    float Point2DMath::dotProduct(CvPoint p1, CvPoint p2)
    {
        CvPoint2D32f u, v;
        // Normalize vectors;
        u = unitVector(p1);
        v = unitVector(p2);
        // Calcultate dot product.
        return u.x*v.x + u.y*v.y;
    }

/*******************************************************************************
* MoveIn::Point2DMath::angle                                                   *
*******************************************************************************/

    float Point2DMath::angle(float dot_product)
    {
        return acos(dot_product) * 180.0 / 3.1415926535897932384;
    }

/*******************************************************************************
* MoveIn::Point2DMath::angle                                                   *
*******************************************************************************/

    float Point2DMath::angle(CvPoint p1,
                             CvPoint p2)
    {
        return angle(dotProduct(p1, p2));
    }

/*******************************************************************************
* MoveIn::Point2DMath::orientedAngle                                           *
*******************************************************************************/

    float Point2DMath::orientedAngle(CvPoint p1,
                                     CvPoint p2)
    {
        if((p1.x * p2.y - p1.y * p2.x) >= 0)
        {
            return angle(dotProduct(p1, p2));
        }
        else
        {
            return 360.0 - angle(dotProduct(p1, p2));
        }
    }

/*******************************************************************************
* MoveIn::Point2DMath::sum                                                     *
*******************************************************************************/

    CvPoint Point2DMath::sum(CvPoint u, CvPoint v)
    {
        CvPoint result;
        result.x = u.x + v.x;
        result.y = u.y + v.y;
        return result;
    }

/*******************************************************************************
* MoveIn::Point2DMath::printVector                                             *
*******************************************************************************/

    void Point2DMath::rotate(CvPoint *start,
                             CvPoint *end,
                             float angle)
    {
        CvPoint result;
        // Translate to origin (0,0).
        end->x -= start->x;
        end->y -= start->y;
        if(angle > 360.0)
        {
            int division = (int) (angle / 360.0);
            angle = angle - (360.0 * division);
        }
        // Degrees to radians.
        angle = CV_PI / 180.0 * angle;
        // Rotate vector.
        result.x = (int) (cos(angle)*end->x - sin(angle)*end->y);
        result.y = (int) (sin(angle)*end->x + cos(angle)*end->y);
        // Translate to original position.
        end->x = result.x + start->x;
        end->y = result.y + start->y;
    }

/*******************************************************************************
* MoveIn::Point2DMath::printVector                                             *
*******************************************************************************/

    void Point2DMath::printVector(RGBImage *source,
                                  CvPoint start,
                                  CvPoint end)
    {
        // Only draw the vector if it is confined inside the source image.
        if(start.x < 0 || end.x < 0 ||
           start.x >= source->getWidth() || end.x >= source->getWidth() ||
           start.y < 0 || end.y < 0 ||
           start.y >= source->getHeight() || end.x >= source->getHeight())
        {
            return;
        }
        // Draw the line that represents the vector.
        cvLine(source->getIplImage(),
               start,
               end,
               CV_RGB(255,255,000),
               2,
               CV_AA,
               0);
        // Draw the tips of the arrow. Do some scaling so that the
        // tips look proportional to the main line of the arrow.
        double angle, hypotenuse;
        angle =
        atan2((double) start.y - end.y, (double) start.x - end.x);
        hypotenuse =
        sqrt(pow((start.y - end.y), 2.0) + pow((start.x - end.x), 2.0));
        start.x = (int) (end.x + 9 * cos(angle + CV_PI / 4));
        start.y = (int) (end.y + 9 * sin(angle + CV_PI / 4));
        cvLine(source->getIplImage(),
               start,
               end,
               CV_RGB(255,255,000),
               2,
               CV_AA,
               0);
        start.x = (int) (end.x + 9 * cos(angle - CV_PI / 4));
        start.y = (int) (end.y + 9 * sin(angle - CV_PI / 4));
        cvLine(source->getIplImage(),
               start,
               end,
               CV_RGB(255,255,000),
               2,
               CV_AA,
               0);
    }
}

#endif


